#include "BlenderNCentrifuge.hpp"
// https://github.com/nbsdx/SimpleJSON
#include "Libs/SimpleJSON/JSON.hpp"
#include "MiscUtils/TdrMisc/SEngine.h"
#include "TdrBLDCMasterCmdBuilder.hpp"
#include "Windows.h"

using namespace std;

BlenderOCentrifugeSlave::BlenderOCentrifugeSlave(GeneralSlave* ref) {
	motorInit(ref);
}

BlenderOCentrifugeSlave::~BlenderOCentrifugeSlave() {
	genSlaveRelease();
	motorRelease();
}

int BlenderOCentrifugeSlave::readOut(uint64_t readIndex) {
	preSendingConfig();
	if (isSimu()) {
		simuReadOut(readIndex);
		Sleep(20);
		return 1;
	}
	else {

	}
	return 0;
}

int BlenderOCentrifugeSlave::setModeSpeed(float newSpeed) {
	preSendingConfig();
	_state._mode = CtrlMode::Speed;
	if (isSimu()) {
		_simuStateTarget.speed = newSpeed;
		_simuStateTarget.torque = newSpeed / 100;
		_simuStateTarget.position = 0;
		_simuStateTarget._mode = CtrlMode::Speed;

		_simuMeta.simuStartTime = clock();

		return 1;
	}
	else {
		//TdrBLDCMasterCmdBuilder::

	}
	return 0;
}

int BlenderOCentrifugeSlave::setModeTorque(float newTorque) {
	preSendingConfig();
	_state._mode = CtrlMode::Torque;
	if (isSimu()) {
		_simuStateTarget.speed = newTorque * 50;
		_simuStateTarget.torque = newTorque;
		_simuStateTarget._mode = CtrlMode::Torque;
		_simuStateTarget.position = 0;

		_simuMeta.simuStartTime = clock();

		return 1;
	}
	else {
		//TdrBLDCMasterCmdBuilder::

	}
	return 0;
}

int BlenderOCentrifugeSlave::setModePosition(float newPos) {
	preSendingConfig();
	_state._mode = CtrlMode::Position;
	if (isSimu()) {
		_state.speed = 0;
		_state.torque = 0;

		_simuStateTarget.position = newPos;
		_simuStateTarget.torque = 10;

		_simuMeta.simuStartTime = clock();
		return 1;
	}
	return 0;
}

int BlenderOCentrifugeSlave::stop(int hardStop) {
	if (hardStop == 0) {
		return setModeSpeed(0);
	}
	else {
		preSendingConfig();
		_state._mode = CtrlMode::Idle;
		if (isSimu()) {
			_state.speed = 0;
			_state.torque = 0;
			return 1;
		}
		else {
			return 0;
		}
	}
}

int BlenderOCentrifugeSlave::moveInMode(std::wstring mode, float distOrRate) {
	if (mode.compare(L"motor_speed_control") == 0) {
		return setModeSpeed(distOrRate);
	}
	else if (mode.compare(L"motor_torque_control") == 0) {
		return setModeTorque(distOrRate);
	}
	else if (mode.compare(L"motor_position_control") == 0) {
		return setModePosition(distOrRate);
	}
	return 0;
}

std::wstring BlenderOCentrifugeSlave::getCurCoordStr() {
	return TWFormater::float2str(_state.position, -1, 3);
}

std::wstring BlenderOCentrifugeSlave::getTargetCoordStr() {
	return TWFormater::float2str(_simuStateTarget.position, -1, 3);
}

//virtual std::wstring getLimitedStr();
std::wstring BlenderOCentrifugeSlave::getSpeedStr() {
	return TWFormater::float2str(_state.speed, -1, 0);
}

std::wstring BlenderOCentrifugeSlave::getLoadStr() {
	return TWFormater::float2str(_state.torque, -1, 2);
}

std::wstring BlenderOCentrifugeSlave::getActionStr() {
	if (_state._mode == CtrlMode::Idle) {
		return L"Idle";
	}
	if (_state._mode == CtrlMode::Speed) {
		return L"Speed_control";
	}
	if (_state._mode == CtrlMode::Torque) {
		return L"Torque_control";
	}
	if (_state._mode == CtrlMode::Position) {
		return L"Position_control";
	}
	return L"Undefined";
}

int BlenderOCentrifugeSlave::simuReadOut(uint64_t readIndex) {
	if (_state._mode == CtrlMode::Idle) {
		return 1;
	}
	int reachTargetTime = 5000;
	int timeElapse = clock() - _simuMeta.simuStartTime;
	int timeSinceLast = clock() - _simuMeta.simuUpdateTime;
	if (timeElapse > reachTargetTime) {
		if (_state._mode == CtrlMode::Position) {
			_state.torque = (rand() % 99 - 49) / 100.0f * _simuStateTarget.torque;
			_state.position = _simuStateTarget.position;
		}
		else {
			_state.speed = _simuStateTarget.speed * (rand() % 10 + 95) / 100;
			_state.torque = _simuStateTarget.torque * (rand() % 10 + 95) / 100;
			float timeMin = timeSinceLast / 60000.0f;
			float phaseAngle = timeMin * _state.speed * 360;
			_state.position = ((int)(_state.position + phaseAngle)) % 360;
		}
	}
	else {
		float ratio = log(1 + timeElapse / 1000.0f) / log(1 + reachTargetTime / 1000.0f);
		if (_state._mode == CtrlMode::Position) {
			_state.position = _state.position * (1 - ratio) + _simuStateTarget.position * ratio;
		}
		else {
			_state.speed = _simuStateTarget.speed * ratio;
			_state.torque = _simuStateTarget.torque * ratio;
			float timeMin = timeSinceLast / 60000.0f;
			float phaseAngle = timeMin * _state.speed * 360;
			_state.position = ((int)(_state.position + phaseAngle)) % 360;

			//TLogger::writeLine("Time since last " + TFormater::float2str(timeSinceLast)
			//	+ ", angle=" + TFormater::float2str(phaseAngle));
		}
	}

	_simuMeta.simuUpdateTime = clock();

	return 1;
}

std::set<std::wstring> BlenderOCentrifugeSlave::getAxialFuncs() {
	std::set<std::wstring> ret;

	//ret.insert(L"speed_control");
	ret.insert(L"position_control");

	return ret;
}
